A Non-Flat Terrain Biped Gait Planner Based on DIRCON

Various constraints exist in bipedal movement. Due to the natural ability of effectively handling constraints, trajectory optimization has become one of the mainstream methods in biped gait planning, especially when constraints become much more complex on non-flat terrain. In this paper, we propose a multi-modal biped gait planner based on DIRCON, which can generate different gaits for multiple, non-flat terrains. Firstly, a virtual knot is designed to model the state transitions when the swing foot contacts terrain and is inserted as the first knot of the target trajectory of the current support phase. Thus, a complete gait or multi-modal gaits sequence can be generated at one time. Then, slacked complementary constraints, which can avoid undesired trajectories, are elaborated to describe the coupling relationships between terrain information and bipedal motion for trajectory optimization based gait planning. The concrete form of the gait planner is also delivered. Finally, we verify the performance of the planner, as well as the structural design of our newly designed biped robot in CoppeliaSim through flat terrain walking, stairs terrain walking and quincuncial piles walking. The three experiments show that the gaits planned by the proposed planner can enable the robot to walk stably over non-flat terrains, even through simple PD control.


3-1301-3903
S
ate Key Laboratory of Robotics and System
Harbin Institute of Technology
150001HarbinChina

Yanhe Zhu 0000-0003-1301-3903
State Key Laboratory of Robotics and System
Harbin Institute of Technology
150001HarbinChina

Jie Zhao 
State Key Laboratory of Robotics and System
Harbin Institute of Technology
150001HarbinChina

A Non-Flat Terrain Biped Gait Planner Based on DIRCON
18 November 2022326D3A4B9EDC73EBB61CD47D1089CDA410.3390/biomimetics7040203Rece ved: 17 October 2022 Accepted: 16 November 2022roboticsbiped gait planningtrajectory optimizationnon-flat terrain
Various constraints exist in bipedal movement.Due to the natural ability of effectively handling constraints, trajectory optimization has become one of the mainstream methods in biped gait planning, especially when constraints become much more complex on non-flat terrain.In this paper, we propose a multi-modal biped gait planner based on DIRCON, which can generate different gaits for multiple, non-flat terrains.Firstly, a virtual knot is designed to model the state transitions when the swing foot contacts terrain and is inserted as the first knot of the target trajectory of the current support phase.Thus, a complete gait or multi-modal gaits sequence can be generated at one time.Then, slacked complementary constraints, which can avoid undesired trajectorie , are elaborated to describe the coupling relationships between terrain information and bipedal motion for trajectory optimization based gait planning.The concrete form of the gait planner is also delivered.Finally, we verify the performance of the planner, as well as the structural design of our newly designed biped robot in CoppeliaSim through flat terrain walking, stairs terrain walking and quincuncial piles walking.The three experiments show that the gaits planned by the proposed planner can enable the robot to walk stably over non-flat terrains, even through simple PD control.

Introduction

Robots are subjected to various constraints during the movement process, such as joint limits, obstacle avoidance and external contact constraints.Because the base of a biped robot floats while the supporting foot must maintain contact with the terrain and not slide, more constraints should be considered in the biped gait planning.Especially when the terrain is non-flat, such as stairs or quincuncial piles, the constraints become more stringent.

Currently, Hybrid Zero Dynamics (HZD) [1][2][3], Reinforce Learning (RL) [4][5][6][7] and Trajectory Optimization (TO) [8][9][10][11][12][13][14][15][16] are the three mainstream methods for the biped gait planning.Because of the natural ability to deal with multifarious constraints, the TO method is particularly suitable for much more complex constraints in biped gait planning [10,12].At the moment, flat terrain gait planning based on TO has produced a number of promising experimental results.Michael Posa et al. [10] used Direct Transcription (DT) to generate a joint-level trajectory that allows the FastRunner to run at 20 MPH without specifying the contact sequence in advance.Hereid et al. [13] introduced an on-line gait planner based on Direct Collocation (DC) without removing or lumping degrees of freedom, which makes the Cassie series robot walk dynamically.

However, as the non-flat terrain further limits robot movement and the requirement of balance maintenance becomes much more difficult, more motion details and constraints need to be considered during biped gait planning to acquire a dynamic, feasible trajectory.Up to now, biped gait planning of non-flat terrain is still an open problem.Li et al. [17] proposed a Linear Inverted Pendulum Model (LIPM)-based non-flat terrain gait planner with the consideration of both the single support phase and double support phase, and the robot can step over a convex terrain.Although the reduced dynamic model, such as the LIPM, can make the robot adapt to non-flat terrain, it is difficult to apply to the large-scale structural non-flat terrain like a staircase or quincuncial piles because of the Biomimetics 2022, 7, 203 2 of 17 requirement of the constant height of center of mass, and only a few motion details can be considered.Kumar et al. [18] trained a feedforward neural network with a gait dataset using support symmetry, and the corresponding gait can be generated online based on different terrain types.Indeed, this method can generate a variety of non-flat terrain gaits, but due to the different mechanic characteristics of biped robots, the general significance of the dataset cannot be guaranteed.The biped gait data of non-flat terrain is still especially scarce presently, so the applicability of learning-based gait planning is limited.A DC-based gait planning method proposed by Winkler et al. [9] utilized phase-based parameterization of the contact force and the motion of the swing foot to plan gaits for quadrupeds, bipeds and monopeds for various terrains with the robot ANYmal.However, as the dynamic model was still reduced, and the contact model between the supporting foot and terrain is also single point, the planner cannot account for enough motion details for a reasonable gait of complicated motions and more complex robots, such as humanoid robots.Based on the direct collocation, Michael Posa et al. [8] proposed the Constrained Direct Collocation (DIRCON), which can perform gait planning on the implicit constrained state space manifold caused by the derivatives of kinematic constraints.Due to the full-order dynamic model, it is introduced as the collocation constraint, a dynamic feasible joint-level trajectory with third-order accuracy that can be obtained, which enables the complex humanoid robot, Altas, to walk at 1 m/s on flat terrain.Because of the engagement of full-order dynamic with the multi-point contact model, and the derivatives of kinematic constraints, DIRCON is a good potential framework for biped gait planning when there is a need of considering more motion details, such as non-flat terrain walking.

During biped robot movement on non-flat terrain, a coupling relationship exists between the swing foot and terrain such that the height of the swing foot has to be greater than the terrain to avoid collision when the foot reaches the position of the terrain.Moreover, some complex constraints which are difficult to express in linear form can be written in complementary form [19][20][21], such as contact trigger conditions and contact force, which is analogous to the coupling relationship between the swing foot and terrain.Inspired by this, we try to use complementary constraints to introduce the relationships between f

rrains into gait planning.The main
ontributions of this paper can be summarized as follows: first, we investigate the coupling relationship of terrain information and the biped foot, and the s ntary constraint which can describe the different relationships and avoid undesired trajectory are proposed for a TO-based gait planner.Second w onstraints and cost function, we extend DIRCON into a non-flat terrain biped gait planner which can generate a complete gait or gait sequence with a specific pattern in single planning.Third, the structural design of a newly designed biped robot by our laboratory is validated with the proposed planner thro reatly promotes the development of a novel biped robot, and the proposed planner and constraints can be used as a prototype for high-level motion planning in general bipedal control architectures.

This paper is organized as follows.In Section 2, we recover the theoretical foundations of TO and DIRCON based gait planner.Section 3 first explains how to use a virtual knot to handle the state transitions when the sw

g foot is touching the terrain, then the coupling relations
ips between bipedal foot motion and terrain are investigated, and the slacked complementary constraints for gait planning are proposed.Simultaneously, we also show how the elaborated constraints can avoid some undesired trajectories, and then we provide the concrete form of the proposed planner.In Section 4, we test the performance of the proposed planner through three different planar walking experiments: flat terrain walking, stairs terrain walking and quincuncial piles walking; meanwhile, the structural design of our robot is also validated.Section 5 summarizes this paper, analyzes the limitations of the planner and illustrates the future work.


Related Work


Constrained Hybrid System Dynamics

The robot is a hybrid dynamic system whose state vector typically contains a general posit ensions vector derived from the active joints, and the general form of robotic system dynamics takes:

.
q(t) = v(t), . v(t) = f(q(t), v(t), u(t));(1)
The biped robot is not only a high-dimensional hybrid dynamic system but also has a constrained floating base.Because the robot needs contact with the terrain or the external environment during movement, contact force should be considered in the dynamic model.Then the dynamics can be further written as:

.
q(t) = v(t), . v(t) = f(q(t), v(t), u(t) , and the dynamic model can be derived rom the da a file of our newly designed robot.It is wor h noting that the corresponding kinematic position constraints of supporting foot ϕ(q) : R n → R c should act at the same points of the contact during the whole gait.


Trajectory Optimization and Constrained Direct Collocation

TO can find a feasible joint trajectory because it is naturally capable of dealing with constraints, particularly in high-dimensional and constrained state space.The DC method is currently widely used in biped gait planning.Through the Simpson integration property, C can insert the dynamics as a collocation constraint at the collocation point, which is usually located at the midpoint between two adjacent knots.The cubic Hermite spline and the first-order curve are utilized to interpolate the state trajectory and the input s follows:
min z l f (X N ) + h N ∑ k=1 l(x k , u k ) s.t. for k = 1, 2, . . . N − 1 g(x k , u k , x k+1 , k+1 ) = 0 m(z) ≤ 0 (3)
where z is the set of decision variables which consists of x k , u k and h.k represents the time index of the state trajectory and the input trajectory, i.e., the kth knot.h is the time step of the adjacent knots and m(z) represents some relevant constraints such as joint limits, input limits, etc. g(z) is the collocation constraint, whose value is equal to the difference between the slope of Hermite spline and robot dynamics at the collocation point of t c,k [20], which is shown as follows:
t c,k = 1 2 (t k + t k+1 ), h = t k+1 − t k , u t c,k = 1 2

, for k = 1, 2, . . . N − 1 x t c,k = 1 2 x
t k + x t k+1 + h 8 .
x t k − .

x t k+1 , .

x t c,k = 3 2h x t k − x t k+1 + 1 4 .

x t k + .

x t k+1 , g(x k , u k , x k+1 , u k+1 ) =
. x t c,k − f x t k , u t c,k(4)
In order to make the planned trajectories conform to the system dynamic, DC requires the state variables (q i , .q i ,q i+1 , .q i+1 ) at the adjacent knots, which satisfy the collocation constraint and the coefficients in the interpolated splines, can be derived from the states of the knots.In other words, the collocation constraint directs the search of optimization to a local optimal solution of the current cost function, which is dynamically feasible.

Posa et al. [9] found that the derivatives of kinematic position constraints will further compress the state space manifold, so DIRCON was proposed to carry out robot motion planning in this constrained manifold.The specific form of DIRCON is shown as follows:
min z l f (X N ) + h N ∑ k=1 l(x k , u k ) s.t. g x k , u k , x k+1 , u k+1 , λ k , λ k+1 , λ k , δ k = . x t c,k − v + J(q t c,k ) T δ k f x, u, λ k = 0, ϕ(q) = 0, . ϕ(q) = φ(q, v) = 0, .. ϕ(q) = ψ q, v, . v = 0, f or k = 1, 2, . . . N − 1 m(z) ≤ 0. (5)
where λ k , λ k and δ k are the contact forces, force correction and velocity correction, respectively.

.

x t c,k is the slope of the spline at the collocation point, which has the same form with the .

x t c,k in Equation ( 4), and the lo ation of the collocation point is set to the midpoint between adjacent knots as per convention.As a result, DIRCON provides us a general framework for the complicated motion planning of implicit constrained manifold and maintains the advantages o constraints, such as friction cone, can be easily introduced which is especially suitable for gait planning of biped robots.


Approaches


Virtual Knots and Multi-Modal Gait Planning

The movement of the biped robot can be regarded as a hybrid system which consists of several single dynamic systems in different phases and the discrete state transitions are triggered by guard conditions.A typical case is that robot will transfer from single support phase to double support phase when the swing foot contacts the ground without slip, and the contact force will cause a sudden change of the state as the swing foot will instantly stop.In order to plan a complete gait (left-right-left) or a sequence of multimodal gaits (combination of gaits with different speeds), the discrete transitions should be explicitly introduced into the planner.When the contact occurs, the gait planning can be divided into two adjacent modes an position is maintained and the velocity abruptly changes, which complies with the impact law during state transition.We use a momentum observer [24] to model the transitions; a virtual knot, which is constrained by the momentum observer, is inserted as the first knot in the trajectory of the current mode.Then, the two mode trajectories can be planned independently.Because of the existence of virtual knots, planners can connect different modes and then plan a complete gait or a gait sequence consisting of multi-modal gaits in a single plan.

For brevity, assuming the target gait consists of two modes, left support phase (LSP) and right support phase (RSP), and the robot is switching from LSP to RSP.The state transitions can be estimated using momentum observer in the following way:
J(q t k ) T Impulse = K O M q t k v post − v pre q post = q pre = q t k (6)
where Impulse is the Ground Reaction Force (GRF) at the t k moment of the swing foot in contact with the ground, v post and v pre are the velocity components in the state vector

Biomimetics 2022, 7, 203 5 of 17 before and after the contact.J q t k is the Jacobian matrix of the contact point.M q t k and the K O are the inertia matrix and diagonal gain matrix, respectively.We assume that the LSP has m I knots of modI and the RSP takes m i+1 knots f mode i+1 .In order to maintain trajectory continuity between modes, the last knot of modI i should be the first knot of mode i+1 so that the whole sequence contains (m i+1 + m i ) − 1 knots.A virtual knot with the form of E uation ( 7) is inserted between two modes.As a result, the whole sequence has (m i+1 + m i ) knots now, and the virtual knot should satisfy the constraints of Equation ( 6). Figure 1 depicts the trajectory articulation between the two modes for one dimension after the insertion.
Virtual knot i = q vir v vir = q pre v post_i(7)
J(q
) Impulse = K M(q )(v − v ) q = q = q (6)
where Impulse is the Ground Reaction Force (GRF) at the t moment of the swing foot in c ntact with the ground, v and v are the velocity components in the state vector before and after the contact.J(q ) is the Jacobian matrix of the contact point.M(q ) and t e K are the inertia matrix and diago al gain matrix, respectively.We assume that the LSP has m knots of mod and the RSP takes m knots of mode .In order to maintain trajectory continuity between modes, the last knot of modI should be the first knot of mode so that the w ole sequence contains (m + m ) − 1 knots.A virtual knot with the form of Equation ( 7) is inserted between two modes.As a result, the whole sequence has (m + m ) knots now, and the virtual knot should satisfy the constraints of Equation ( 6). Figure 1 depicts the trajectory articulation between the two modes for one dimension after the insertion.The virtual knot establishes the relationship between two modes when contact happens, thus, t

.The following are the main steps of the D
RCON with virtual knots:
𝑉𝑖𝑟𝑡𝑢𝑎𝑙 𝑘𝑛𝑜𝑡 = 𝑞 𝑣 = 𝑞 𝑣 _ (7)
Step 1: Determine the mode sequence according to the contact condition and the robot state.Then establish optimization program based on DIRCON as Equation ( 5).Turn to Step 2 for virtual knots insertion;

Step 2: Check if current mode happens with contact, if not, stay at Step 2 for next step, or add optimization decision variables Impulse and v _ , respectively and turn to Step 3. If current mode is the last, turn to Step 4;

Step 3: Insert the virtual knots as the initial knot of mode and organize the equation constraints of the Impulse and the v _ as Equation ( 6).If mode is not the last mode, return to Step 2, or the insertion has completed and go to Step 4; The virtual knot establishes the relationship between two modes when contact happens, thus, the planner can plan the independent trajectory for each mode.The following are the main steps of the DIRCON with virtual knots:

Step 1: Determine the mode sequence according to the contact condition and the robot state.Then establish optimization program based on DIRCON as Equation ( 5).Turn to Step 2 for virtual knots insertion;

Step 2: Check if current mode happens with contact, if not, stay at Step 2 for next step, or add optimization decision variables Impulse i and v post_i , respectively and turn to Step 3. If current mode is the last, turn to Step 4;

Step 3: Insert the virtual knots as the initial knot of mode i and organize the equation constraints of the Impulse i and the v post_i as Equation ( 6).If mode i is not the last mode, return to Step 2, or the insertion has completed and go to Step 4;

Step 4: Solve the optimization problem, and restore the joint level trajectories and the input trajectories

of Support Location Selection

The location of the supporting foot and t
e obstacle avoidance of the swing foot are two critical keys during biped motion on non-flat terrain and we will illustrate how the proposed constraints introduce them into the planner, respectively.For brevity, a four-step movement of a seven-link XOZ planar biped robot is used as an example.The selection of foot location is depicted in Figure 2, where the robot foot can be considered as a linear foot with length l, the support terrain consists of four rectangles from a 0 to d 1 and x i is the center of the supporti g foot.The orange and green area are defined as dangerous supporting terrain of length L that the robot is considered to be at risk of falling when the supporting foot is located here.Therefore, the safety requirement of foot location is to guarantee each selection is within the blue area.

two critical keys during biped motion on non-flat terrain and we will illustrate ho proposed constraints introduce them into the planner, respectively.For brevity

t location is depicted in Figure 2, where the robot foot can be considere
linear foot with length l, the support terrain consists of four rectangles from a and x is the center of the supporting foot.The orange and green area are defined a gerous supporting terrain of length L that the robot is considered to be at risk of when the supporting foot is located here.Therefore, the safety requirement of foot lo is to guarantee each selection is within the blue area.Assuming the selection of mode is already safe, the three subsequent sele must satisfy the linear inequalities as shown in Equation ( 8) due to the safety require  + 0.5 ≤  ≤  − 0.5  + 0.5 ≤  ≤  − 0.5  + 0.5 ≤  ≤  − 0.5
z x o a 0 a 1 b 0 b 1 c 0 c 1 L L R L R L mode 0 mode 1 mode 2 mode 3 L R l d 0 d 1 L R x 0 x 1 x 2 x 3

Slacked Complementary Constraint of Obstacle Avoidance of the Swing Foot

After foot location selection satisfies the safety requirement, we need to design straint to keep the swing foot from colliding with the terrain or obstacles.The swin obstacle avoidance is more complicated than foot location selection and the coupli lationship can be divided into three situations according to the height of target terra current supporting terrain: swing to flat terrain, swing to higher terrain, and sw lower terrain.As shown in Figure 3, we continue to utilize the seven-link planar model as example.

a. Swing to higher terrain When the robot swings its foot to a terrain higher than the current supporting t the front and rear edges which reach the position of the ter ain must be higher th terrain height to avoid collision.Three typical trajectories that satisfy the requirem If the supporting foot is located in green or orange area, robot will be at the risk of falling.

Assuming the selection of mode 0 is already safe, the three subsequent selections must satisfy the linear inequalities as shown in Equation ( 8) due to the safety requirement.
b 0 + 0.5l ≤ x 1 ≤ b 1 − 0.5l c 0 + 0.5l ≤ x 2 ≤ c 1 − 0.5l d 0 + 0.5l ≤ x 3 ≤ d 1 − 0.5l (8)

Slacked Complementary Constraint of Obstacle Avoidance of the Swing Foot

After foot location selection satisfies the safety requirement, we need to design a constraint to keep the swing foot from colliding with the terrain or obstacles.The swing foot obstacle avoida e divided into three situations according to the height of target terrain and current supporting terrain: swing to flat terrain, swing to higher terrain, and swing to lower terrain.As shown in Figure 3, we continue to utilize the seven-link planar robot model as example.a.

Swing to higher terrain When the robot swings its foot to a terrain higher than the current supporting terrain, the front and rear edges which reach the position of the terrain must be higher than the terrain height to avoid collision.Three typical trajectories that satisfy the requirement of collision avoidance of higher terrain are shown in Figure 3a.The dashed gray trajectory first raises the foot higher than the target terrain and then moves horizontally and makes a rapid descent at the end.The black trajectory moves horizontally first, then rapidly lifts, and finally, rapidly descends, and the green trajectory behaves more mildly without significant ascent or descent.All the trajectories satisfy the requirement of collision avoidance as shown in Equation (9).Actually, we frequently select the green as a desired trajectory due to the smoothness, safety and balance maintenance of the biped robot.

x font_end ≥ e 1 z font_end > d 4 x back_end ≥ e 1 z back_end > d 4 (9) Directly utilizing the complementary form, indeed, can guarantee the swing foot passes through the obstacle, but it cannot avoid or reduce the occurrence of the black and gray trajectories.Setting a reasonable cost function of joint input to avoid a huge acceleration is a possible method, however, the value of weight can only be determined through ample real machine experiments, and the process has to keep repeating when any parameter or terrain is changed.In order to induce the solver to converge to the green trajectory, we introduce additional slack variables and reform the complementary constraint of the front edge as follows:
(x + 0.5l − e 1 + slack 0 )(z − d 4 * ) ≥ 0, (e 1 − e 0 ) > slack 0 ≥ c(e 1 − e 0 ) (10)
collision avoidance of higher terrain are shown in Figure 3a.The dashed gray trajectory first raises the foot higher than the target terrain and then moves horizontally and makes a rapid descent at the end.The black trajectory moves horizontally first, then rapidly lifts, and finally, rapidly descends, and the green trajectory behaves more mildly without significant a on avoidance as shown in Equation (9).Actually, we frequently select the green as a desired trajectory due to the smoothness, safety and balance maintenance of the biped robot.Directly utilizing the complementary form, indeed, can guarantee the swing foot passes through the obstacle, but it cannot avoid or reduce the occurrence of the black and gray trajectories.Setting a reasonable cost function of joint input to avoid a huge acceleration is a possible method, however, the value of weight can only be determined through ample real machine experiments, and the process has to keep repeating when any parameter or terrain is c troduce additional slack variables and reform the complementary constraint of the front edge as follows: As shown in Figure 3a, x represents the center coordinate of the swing foot and slack 0 is the slack decision variable to scale the distance between the proximal end of the terrain and the swing foot.c is a positive decimal smaller than 1, e 1 and e 0 are the coordinates of the supporting terrain and target terrain, respectively, and d 4 * is a slacked safety height slightly larger than d 4 .This elaborate slacked complementary constraint implies that when the front edge of the foot reaches the point of e 1 − slack 0 , its height will be greater than the slacked safe height d 4 * .Therefore, adding a term of the slack variable slack 0 to the cost function will induce the solver to converge to the desired pattern traject ry of the adjacent terrains and slack 0 .Then, the green trajectory can be easily obtained.The constraint of rear edge takes the same form with the front edge and the planner will successfully generate a target gait so that the robot steps to higher terrain safely.
(x + 0.5l − e + slack )(z − d * ) ≥ 0, (e − e ) > slack ≥ c(e − e )(10)

b. Swing to lower and flat terrain

The situation changes when the heigh , considering the requirement of balance maintenance and the purpose of reducing the horizontal speed when robot foot is in contact with the terrain, the green trajectory in Figure 3b is more reasonable than the gray and the black.Although the black may occur, it definitely not what we want, and the gray trajectory is acceptable when the contact force does not exceed the friction limitation.To eliminate the black-like trajectory, we can also use the slacked complementary constraint in the following form:
−(x − (b 0 + slack 1 ))(z − d 5 * ) ≥ 0, b 1 − b 0 − 0.5l > slack 1 > 0.5l(11)
The purpose of the slack variable slack 1 is same with Equation ( 10), b 0 and b 1 are the terrain parameters shown in Figure 3b.Through the constraint, the solver ensures that the height of the swing foot does not fall below d 5 * when its x coordinate does not exceed b 0 + slack 1 .Compared with constraint of the higher terrain, the sign of Equation ( 11) is opposite due to the target terrain being lower th

the supporting terrain, and the black trajectory can
be directly avoided even without the effort of a cost function.Due to the supporting terrain, the same trick can be implemented as Equation (12), where the edge should be higher than the supporting terrain until approaching the contact point, which is determined by slack 1 .With the assistance of Equation ( 8), the solver can easily converge to the desired gait.
−(x − (b 0 + slack 2 )) z − d ˆ5 ≥ 0, slack 2 = c * slack 1(12)
When the height o oot is just greater than the terrain is efficient and the complementary form of flat terrain here is just to keep consistent with the above description due to the types of structural terrains.It is worth noting that we only take the single step movement of a 2D planar biped robot as an example to introduce how the slacked complementary constraint describes the relationship between terrain and biped foot.It is easy to expand from 2D single step to 3D multiple steps because the only needed work is repetition.As the terrain information is the only factor influencing gait pattern, we can generate a multi-step sequence from single step planning when terrain is periodic, such as stairs.


Hybrid DIRCON with Slacked Complementary Constraints

Now we present the complete form of the proposed DIRCON-based non-flat terrain gait planner.As an example, the robot would take three modes to step over two identical stairs and the robot motion is similar to Figures 2 and 3a.Three modes and two contacts exist in the movement, and the entire trajectory consists of N knots after inserting two virtual knots at the beginning of mode 1 and mode 2 .The planner finally takes the following form:
min z l f (x N ) + h i N ∑ k=1 l(x k , u k ) + K high (slack 0 − c(e 1 − e 0 )) 2 + K N ∑ k=1 L(x k , x k−1 , u k , u k−1 ) s.t. g x k , u k , x k+1 , u k+1 , λ k λ k+1 , λ k , δ k = . x t c,k − v + J q t c,k δ k f x k , u k , λ k = 0, for m in {mode 0 , mode 1 , mode 2 } ϕ m (q k ) = 0, . ϕ m (q) = φ(q k , v k ) = 0, .. ϕ m (q) = ψ q k , v k , u k , λ k = 0, foot selection constraint m = X k − b + 0 ≥ 0 − X k − b − 1 ≥ 0 , swing foot constraint m =    X k − (e 1 − slack 0 ) Z k − d 4 * ≥ 0 (e 1 − e 0 ) > slack 0 > c(e 1 − e 0 ) c = 0.5 , for . . . in {10irtual, 10irtual} Virtual knots constraints i =    q vir_i = q pre_i v vir_i = v pre_i + M q pre_i −1 K −1 O J(q pre_i ) T Impulse i for k = 1, 2, . . . N − 1 m(x k , . . .) ≤ 0 Contact friction constraints Periodic constraints Duration constraint Other constraints (14)
where z is the set of all decision variables, x k and u k are the robot state and joint input at the kth knot.h i are the decision variables of all the time steps between the two adjacent knots.K high is the weight of trajectory induction term.X, Z and X k , Z k are the slack variables of the supporting foot coordinate and swing foot coordinate at the kth knot in mode I , respectively.q vir_i and v pre_i are the state of the virtual knot at ith contact.q pre i , v pre i is the original knot before the contact and Impulse i is the GRF.In terms of constraints, g(•) is the full order dynamic collocation constraint of DIRCON with multi-point contact model, ϕ m (q), .ϕ m (q), .. ϕ m (q) are the kinematic position constraints and their derivatives, respectively, foot selection constraint and floating foot constraint as described in Section 3.2 and the parameters, such as d 4 * , should be tuned according to the terrai

height of di
ferent modes.Virtual knots constraints are used for the inserted knots at the beginning of mode 1 and mode 2 as illustrated in Section 3.1.m(z) is the retainable constraint representing joint li

ts, velocity limits and input limits on th
whole trajectory.Contact friction constraints and Periodic constraints are the friction cone constraint of multi points contact model and the gait periodic constraint, respectively.The duration constraint maintains that the total duration of mode 0 and mode 2 is equal to mode 1 , and the total duration is also constrained to 1.0 s.Other constraints include the constraints of gait pattern such as the max height of the swing foot, gait speed, upward increment of COM, etc. Due to the cost function, the first two terms are the final state cost and the process state cost during the movement.The third term is the swing foot trajectory induction term and the last term is the trajectory smoothing cost, which is the squares of the difference of robot state and input decision variables in adjacent knots.Due to the contact, we have not predetermined the exact time of contact, and instead we specified a contact sequence.During the complete gait of LSP-RSP-LSP, the contacts occur at the final knot of mode 0 and mode 1 , respectively.The exact time of the contact are determined by the planner according to the duration constraint and gait speed constraint.It is worth noting that the number of slacked complementary constraints of the floating foot is twice the number of knots due to the two points contact model, and the number of foot location constraints is equal to the number of knots, because we set the constraint to the mid-point of the supporting foot which is not supposed to move.As the constraints should always be satisfied, all the slacked constraints should act on each knot.After solving the optimization, the joint state and input trajectory can be directly obtained.


Experim

ts

In this
ection, the proposed planner as shown in Equation ( 14) is verified through three planar walking simulations: flat terrain walking, stairs terrain walking quincuncial piles walking.


Robot Mode

and Details of Implementation

The robot m
del used for the simulation is a newly designed biped robot by our laboratory which consists of five parts: base, hip, thigh, shank and foot.Each leg has five active joints, three for the hip, one for the knee and one for the ankle.The connector of joint is made of aluminum alloy, and the main body is made of high-strength carbon fiber.As shown in Figure 4, the robot stands 1.2 m tall and weighs 13.5 kg.The design weight of the base is 3.0 kg plus 2.7 kg for a single hip, which consists of three identical active joints.The thigh weight is 1.1 kg, 1.3 kg for the shank and 0.15 kg for the foot.The robot state space of the robot is 24-dimensional, namely 12-dimensional general position and 12-dimensional general velocity.More technical details of the biped robot will be explained in another paper.contact, we have not predetermined the exact time of contact, and instead we specified a contact sequence.During the complete gait of LSP-RSP-LSP, the contacts occur at the final knot of mode and mode , respectively.The exact time of the contact are determined by the planner according to the duration constraint and gait speed constraint.It is worth noting that the number of slacked complementary constraints of the floating foot is twice the number of knots due to the two points contact model, and the number of foot location constraints is equal to the number of knots, because we set the constraint to the mid-point of the supporting foot which is not supposed to move.As the constraints should always be satisfied, all the slacked constraints should act on each knot.After solving the optimization, the joint state and input trajectory can be directly obtained.


Experiments

In this section, the proposed planner as shown in Equation ( 14) is verified through three planar walking simulations: flat terrain walking, stairs terrain walking and quincuncial piles walking.


Robot Model and Details of Implementation

The robot model used for the simulation is a newly designed biped robot by our laboratory which consists of five parts: base, hip, thigh, shank and foot.Each leg has five active joints, three for the hip, one for the knee and one for the ankle.The connector of joint is made of aluminum alloy, and the main body is made of high-strength carbon fiber.As shown in Figure 4, the robot stands 1.2 m tall and weighs 13.5 kg.The design weight of the base is 3.0 kg plus 2.7 kg for a single hip, which consists of three identical active joints.The thigh weight is 1.1 kg, 1.3 kg for the shank and 0.15 kg for the foot.The robot state space of the robot is 24-dimensional, namely 12-dimensional general position and 12-dimensional general velocity.More technical details of the biped robot will be explained in another paper.Due to the details of implementation, our hardware used to solve the optimization is "Intel Core i7 9750H" and the software environment is Ubuntu 18.04.C++ and CMake are used as implementation language and compilation tool, respectively.Coppeliasim 4.2.0 [25] is used as the simulation platform and the dynamic engine "Newton" implements the physics.We utilize a robot toolbox "Drake" [26] from MIT to implement the planner that the plant parser is used to establish the dynamic model and the framework of mathematic program is engaged for building the planner.An efficient nonlinear program solver "SNOPT" is used to solve the optimization.Normalized random trajectories, which are modulated according to the parameters of the robot structural design, are introduced as the initial gu

s.As for other priors
the max joint torques should not exceed 10 Nm, the velocities are also smaller than 6.28 rad/s and the GRF is initialized to the gravity of the robot.After solving, the joint state trajectories and input trajectories are restored and a sample of 100 Hz is conducted to acquire the target gait.A control program which sends the gait and triggers the simulation step-by-step is completed through the Remote API of CoppeliaSim, the robot model files are parsed and the robot is installed into the simulation by a plugin.The control mode of each active joint is set to PD position control and the friction coefficient of terrain is set to 0.6.The time step of the dynamic engine is also set to 10 ms, which matches the frequency of the sample of trajectories.It is noting that the dynamic model in the planner is planar that three floating coordinates exist, namely 'X', 'Z' and 'Pitch', and correspondingly, the simulation is also 2D.In order to realize sagittal plane simulation, some constraints are implemented to shun the motion of the coronal plane.A small mass link (0.005 kg) is fixed to the world frame and another two small mass links (0.01 kg), as well as the baselink of the robot, are attached by two prismatic joints and one continuous revolute joint, respectively, and the cascade is shown in the frames of walking.The three joints realize the floating degrees of freedom of sagittal plane.The mass of three links are the experienced values which cannot be zero due to the unknown behavior of physics engine of zero-mass link.Finally, the gait is tracked in simulation and enables the robot to execute planar walking on different terrains.


Flat Terrain Walking

The first experiment is that the robot walks on flat terrain along the X direction at a speed of 0.5 m/s.At this time, the planner disables the fo

location selectio
constraint and the swing foot constraint.The target gait is designed as a three mode sequence (LSP-RSP-LSP) which consists of 38 knots and 2 virtual knots.The first mode has 10 knots, and 20 knots are in the second.The total duration is constrained to less than 1.0 s.The second LSP also has 10 knots due to the symmetry.Furthermore, a symmetry constraint of the supporting foot location is inserted to ensure that the left support phase and right support phase take the same step length.The quality of the prior trajectory is one of the major factors influencing the performance of planner.We use a normalized random prior, and the SNOPT solving time ranges from 120 s to 3 min.Figure 5 depicts the joint input trajectory generated by the planner, while the target position trajectory is shown in Figure 6 as blue.The maximum joint input is 18.5 Nm and the joint position trajectory is relatively smooth, which can be tracked easily.We use simple PD position control to track the planned trajectory and the performance is shown in Figure 6 as red.Because the collocation constraint in the planner employs a full-order dynamic model, even PD control can track the target trajectory well.The simulation shows that the planned trajectory can keep the robot walking smoothly for all of the simulation time.Figure 7 gives five key gait frames of walking on flat terrain which consists of right foot lifting, stepping forward, right foot contacting the ground, left foot lifting and stepping forward, and these motions form a complete periodic gait.The full simulation can be found in the first section of the supplementary mate ials.


Stepping Upstairs

Walking on stairs is the second experiment.Assume we have obtained the information of stair terrain through some other technical methods that the width and height are 3 cm and 22 cm, respectively.In contrast to walking on flat ground, the planner enables the supporting foot location constraints and swing foot constraints.We also modified the constraint on the height of the robot COM, as well as kinematic position constraints.That is, the height of the robot COM rises 6 cm in Z direction for each step and the supporting foot raises 3 cm for each contact, while the gait speed in X direction remains 0.5 m/s.Other constraints are consistent with the experiment on flat ground.The average time of solving the entire optimization is about 5.5 min.Figures 8 and 9 show the fin

planned joint position tr
jectory and input trajectory.The peak value of joint input is 34.7 Nm at this time, which is greater than when walking on flat terrain walking but still less than the rated output torque of the motor (50.0 Nm).Furthermore, although we have tried to adjust the weight of the smoothing term in the cost function, the joint position trajectory cannot behave as smoothly as the gait on flat terrain.The robot continues to track the target trajectory using PD position control.Figure 10 depicts the five key frames of the gait which have the same stepping sequence as Figure 7.According to the simulation, the proposed planner can provide a feasible stair terrain gait which allows the robot to step up the stairs stably.The entire experiment can be found in the second section of the supplementary materials.

the entire optimization is about 5.5 min.Figures 8 and 9 show the final plann sition trajectory and input trajectory.The peak value of joint input is 34.7 Nm which is greater than when walking on flat terrain walking but still less tha output torque of the motor (50.0 Nm).Furthermore, although we have tried t weight of the smoothing term in the cost function, the joint position trajectory have as smoothly as the gait on flat terrain.The robot continues to track the t tory using PD position control.Figure 10 depicts the five key frames of the have the same stepping sequence as Figure 7.According to the simulation, th planner can provide a feasible stair terrain gait which allows the robot to step stabl

The entire experiment can
e found in the second section of the sup materials.


Quincuncial Piles Walking

The quincuncial piles gait planning is more complicated as the constraints become much more strict, due to managing the narrow supporting area, and ensuring the swing foot does not collide with the terrain at same time.This experiment validates not only the effectiveness of the foot location selection constraint and swing foot constraint in a strict manner, but also the ability to avoid non-ideal trajectory.The distance between two piles is 11 cm, and each pile is 25 cm in length and 14 cm in width.At this point, the planner must adjust the corresponding parameters of the foot location selection constraint and the swing foot constraint based on the terrain information.In addition, we release the gait speed requirement, so that the forward speed in the X direction is just constrained to greater than or equal to 0.5 m/s.After around 5 min of solving, the solver successfully found a trajectory with a speed of 0.59 m/s. Figure 11 depicts the planned joint input trajectory and the peak torque of active joint is around 30.0 Nm, which is smaller than the rated torque; the joint position trajectory in Figure 12 is relatively smooth.The five key frames of quincuncial piles walking are shown in Figure 13, and the gait is similar to the flat terrain walking as the complementary constraint is equivalent to the simple height constraint of the swinging foot on flat terrain as demonstrated in Section 3.3.The entire experimenta video is available in the third section of the supplementary material.


Quincuncial Piles Walking

The quincuncial piles gait pl

re strict, due to managing
he narrow supporting area, and ensuring the swing foot does not collide with the terrain at same time.This experiment validates not only the effectiveness of the foot location selection constraint and swing foot constraint in a strict manner, but also the ability to avoid non-ideal trajectory.The distance between two piles is 11 cm, and each pile is 25 cm in length and 14 cm in width.At this point, the planner must adjust the corresponding parameters of the foot location selection constraint and the swing foot constraint based on the terrain information.In addition, we release the gait speed requirement, so that the forward speed in the X direction is just constrained to greater than or equal to 0.5 m/s.After around 5 min of solving, the solver successfully found a trajectory with a speed of 0.59 m/s. Figure 11 depicts the planned joint input trajectory and the peak torque of active joint is around 30.0 Nm, which is smaller than the rated torque; the joint position trajectory in Figure 12 is relatively smooth.The five key frames of quincuncial piles walking are shown in Figure 13, and the gait is similar to the flat terrain walking as the complementary constraint is equivalent to the simple height constraint of the swinging foot on flat terrain as demons